#include <cmath>
#include "IntegralHist.h"


//////////////////////////////////////////////////////////////////
// IntegralHist Object											//
//////////////////////////////////////////////////////////////////

// Constructor for integral histogram. Note that the histogram is
// constructed differently for colour and greyscale images.
IntegralHist::IntegralHist(Image* img):
_count(0)
{
	// Set histogram dimensions.
	_d = ORIENTATION_BINS;
	_w = img->GetWidth() - 2;
	_h = img->GetHeight() - 2;

	// Allocate memory for the integral histogram and also for the
	// previous row of the integral histogram.
	_histdata = new float[ORIENTATION_BINS*_w*_h];
	float* prev_row = new float[ORIENTATION_BINS*_w];	// This has to be deleted later.

	// Initialise a pointer at the beginning of the histogram
	float* histdata_p = _histdata;

	if(img->IsGreyscale())
	{
		// Edge detection for grey image.
		for(int i=0; i<_h; i++)
		{
			// Initialise pointers in a 3x3 neighbourhood
			BYTE* tl = static_cast<BYTE*>(img->GetPixelAddress(0,i));
			BYTE* tc = static_cast<BYTE*>(img->GetPixelAddress(1,i));	// BYTE* tc = tl; ++++++tc;
			BYTE* tr = static_cast<BYTE*>(img->GetPixelAddress(2,i));	// BYTE* tr = tc; ++++++tr;
			BYTE* ml = static_cast<BYTE*>(img->GetPixelAddress(0,i+1));
			BYTE* mc = static_cast<BYTE*>(img->GetPixelAddress(1,i+1));	// BYTE* mc = ml; ++++++mc;
			BYTE* mr = static_cast<BYTE*>(img->GetPixelAddress(2,i+1));	// BYTE* mr = mc; ++++++mr;
			BYTE* bl = static_cast<BYTE*>(img->GetPixelAddress(0,i+2));
			BYTE* bc = static_cast<BYTE*>(img->GetPixelAddress(1,i+2));	// BYTE* bc = bl; ++++++bc;
			BYTE* br = static_cast<BYTE*>(img->GetPixelAddress(2,i+2));	// BYTE* br = bc; ++++++br;

			// Allocate memory for the row accumulator and initialise values to zero.
			float* s = new float[ORIENTATION_BINS];				// This has to be deleted later

			for(int k=0; k<ORIENTATION_BINS; k++)
				s[k] = 0;

			for(int j=0; j<_w; j++)
			{
				// If this is the first row, then initialise the
				// previous values to zero.
				if(i == 0)
					for(int k=0; k<ORIENTATION_BINS; k++)
						prev_row[(j*ORIENTATION_BINS)+k] = 0;

				// Calculate edge response and orientation.
				int Gx =	 (*tl) + 2*(*ml) + (*bl)
							-(*tr) - 2*(*mr) - (*br);
				int Gy =	 (*tl) + 2*(*tc) + (*tr)
							-(*bl) - 2*(*bc) - (*br);
				float G = sqrt(static_cast<float>((Gx*Gx)+(Gy*Gy)));
				float theta = 0.0f;

				// Calculate the orientation and work out which 
				// bin to put the pixel in. Note that "atan"
				// returns angles from -pi/2 to pi/2, while 
				// "atan2" has a range from -pi to pi.
				int bin = 0;
				if(ALL_ORIENTATIONS)
				{
					theta = atan2(static_cast<float>(Gy),static_cast<float>(Gx));
					for(int k=0; k<ORIENTATION_BINS; k++)
					{
						// In this situation, values go from -pi to pi.
						if(	(theta > ((k*2*PI)/ORIENTATION_BINS - PI)) &&
							(theta <= (((k+1)*2*PI)/ORIENTATION_BINS - PI))	)
						{
							bin = k;
							break;
						}
					}
				}
				else
				{
					theta = atan(static_cast<float>(Gy)/Gx);
					for(int k=0; k<ORIENTATION_BINS; k++)
					{
						// In this situation, values go from -pi/2 to pi/2.
						if(	(theta > ((k*PI)/ORIENTATION_BINS - PI/2)) &&
							(theta <= (((k+1)*PI)/ORIENTATION_BINS - PI/2))	)
						{
							bin = k;
							break;
						}
					}
				}

				// Increment the integral histogram by the edge
				// magnitude for the correct bin.
				for(int k=0; k<ORIENTATION_BINS; k++)
				{
					if(bin == k)
						s[k] += G;
					(*histdata_p) = s[k] + prev_row[(j*ORIENTATION_BINS)+k];
					prev_row[(j*ORIENTATION_BINS)+k] = (*histdata_p);
					++histdata_p;
				}

				// Image pointers have to be incremented
				++tl;	++tc;	++tr;
				++ml;	++mc;	++mr;
				++bl;	++bc;	++br;
			}
			// Clean up any dynamic memory
			delete [] s;
		}

		// Clean up any dynamic memory
		delete [] prev_row;
	}
	else
	{
		// Edge detection for a colour image. Detection is
		// carried out in each colour plane, and the maximum
		// response is used for each pixel.
		for(int i=0; i<_h; i++)
		{
			// Initialise pointers in a 3x3 neighbourhood
			BYTE* tl = static_cast<BYTE*>(img->GetPixelAddress(0,i));
			BYTE* tc = static_cast<BYTE*>(img->GetPixelAddress(1,i));	// BYTE* tc = tl; ++++++tc;
			BYTE* tr = static_cast<BYTE*>(img->GetPixelAddress(2,i));	// BYTE* tr = tc; ++++++tr;
			BYTE* ml = static_cast<BYTE*>(img->GetPixelAddress(0,i+1));
			BYTE* mc = static_cast<BYTE*>(img->GetPixelAddress(1,i+1));	// BYTE* mc = ml; ++++++mc;
			BYTE* mr = static_cast<BYTE*>(img->GetPixelAddress(2,i+1));	// BYTE* mr = mc; ++++++mr;
			BYTE* bl = static_cast<BYTE*>(img->GetPixelAddress(0,i+2));
			BYTE* bc = static_cast<BYTE*>(img->GetPixelAddress(1,i+2));	// BYTE* bc = bl; ++++++bc;
			BYTE* br = static_cast<BYTE*>(img->GetPixelAddress(2,i+2));	// BYTE* br = bc; ++++++br;

			// Allocate memory for the row accumulator and initialise
			// values to zero.
			float* s = new float[ORIENTATION_BINS];				// This has to be deleted later

			for(int k=0; k<ORIENTATION_BINS; k++)
				s[k] = 0;

			for(int j=0; j<_w; j++)
			{
				// If this is the first row, then initialise the
				// previous values to zero.
				if(i == 0)
					for(int k=0; k<ORIENTATION_BINS; k++)
						prev_row[(j*ORIENTATION_BINS)+k] = 0;

				// Calculate edge response and orientation.
				int Gx = 0;
				int Gy = 0;
				float G = -1.0f;
				float theta = 0.0f;
				for(int l=0; l<3; l++)
				{
					int Gx_temp =	 (*tl) + 2*(*ml) + (*bl)
									-(*tr) - 2*(*mr) - (*br);
					int Gy_temp =	 (*tl) + 2*(*tc) + (*tr)
									-(*bl) - 2*(*bc) - (*br);

					float G_temp = sqrt(static_cast<float>((Gx_temp*Gx_temp)+(Gy_temp*Gy_temp)));
					if(G_temp > G)
					{
						Gx = Gx_temp;
						Gy = Gy_temp; 
						G = G_temp;
					}

					++tl;	++tc;	++tr;
					++ml;	++mc;	++mr;
					++bl;	++bc;	++br;
				}

				// Calculate the orientation and work out which 
				// bin to put the pixel in. Note that "atan"
				// returns angles from -pi/2 to pi/2, while 
				// "atan2" has a range from -pi to pi.
				int bin = 0;
				if(ALL_ORIENTATIONS)
				{
					theta = atan2(static_cast<float>(Gy),static_cast<float>(Gx));
					for(int k=0; k<ORIENTATION_BINS; k++)
					{
						// In this situation, values go from -pi to pi.
						if(	(theta > ((k*2*PI)/ORIENTATION_BINS - PI)) &&
							(theta <= (((k+1)*2*PI)/ORIENTATION_BINS - PI))	)
						{
							bin = k;
							break;
						}
					}
				}
				else
				{
					theta = atan(static_cast<float>(Gy)/Gx);
					for(int k=0; k<ORIENTATION_BINS; k++)
					{
						// In this situation, values go from -pi/2 to pi/2.
						if(	(theta > ((k*PI)/ORIENTATION_BINS - PI/2)) &&
							(theta <= (((k+1)*PI)/ORIENTATION_BINS - PI/2))	)
						{
							bin = k;
							break;
						}
					}
				}

				// Increment the integral histogram by the edge
				// magnitude for the correct bin.
				for(int k=0; k<ORIENTATION_BINS; k++)
				{
					if(bin == k)
						s[k] += G;
					(*histdata_p) = s[k] + prev_row[(j*ORIENTATION_BINS)+k];
					prev_row[(j*ORIENTATION_BINS)+k] = (*histdata_p);
					++histdata_p;
				}
			}
			// Clean up any dynamic memory
			delete [] s;
		}

		// Clean up any dynamic memory
		delete [] prev_row;
	}
}

// Copy constructor for integral histogram.
IntegralHist::IntegralHist(const IntegralHist &init)
{
	// Set count (only used by "smart" pointer).
	_count = init._count;

	// Set histogram dimensions
	_d = init._d;
	_w = init._w;
	_h = init._h;

	_histdata = new float[ORIENTATION_BINS*_w*_h];

	float* histdata_p = _histdata;
	float* histdata_p_init = init._histdata;
	for(int i=0; i<ORIENTATION_BINS*_w*_h; i++)
		*histdata_p++ = *histdata_p_init++;
}

// Destructor for integral histogram.
IntegralHist::~IntegralHist()
{
	delete [] _histdata;
}

//////////////////////////////////////////////////////////////////
// IntegralHistPtr Object										//
//////////////////////////////////////////////////////////////////

// Constructor for smart pointer to an integral histogram.
IntegralHistPtr::IntegralHistPtr(IntegralHist* p): 
_p(p) 
{ 
	if(_p != NULL)
		++_p->_count; 
}

// Copy constructor
IntegralHistPtr::IntegralHistPtr(IntegralHistPtr const& init): 
_p(init._p) 
{ 
	if(_p != NULL)
		++_p->_count; 
}

// Destructor
IntegralHistPtr::~IntegralHistPtr() 
{
	if(_p != NULL)
		if(--_p->_count == 0) 
			delete _p; 
}

// Member pointer operator
IntegralHist* IntegralHistPtr::operator->() 
{ 
	return _p; 
}

// Reference operator
IntegralHist& IntegralHistPtr::operator*() 
{ 
	return *_p; 
}

// Assignment operator
IntegralHistPtr& IntegralHistPtr::operator= (IntegralHistPtr const& p)
{
		IntegralHist* const old = _p;
		_p = p._p;
		if(_p != NULL)
			++_p->_count;
		if(old != NULL)
			if(--old->_count == 0) 
				delete old;
		return *this;
}

// Equal to operator
bool IntegralHistPtr::operator==(const IntegralHistPtr &r) const
{
	return (_p == r._p);
}

// Not equal to operator
bool IntegralHistPtr::operator!=(const IntegralHistPtr &r) const
{
	return (_p != r._p);
}